#include "SpatialIndex.h"
//#include "Classes.h"
#include "DataStructure.h"
//#include "Cluster.h"
#include "vcg/complex/trimesh/base.h"
#include "minSphere.h"
//#include "expand.h"
#include "math.h"
//#include <typeinfo>
//#include "vcg/complex/trimesh/allocate.h"
//#include "vcg/complex/trimesh/tri::Allocator< AllocateMeshType >::NameTypeBound_Base"


//da togliere
/*#include "visualizer.h"
#include "htimer.h"
#include "minSphere.h"*/


SpatialIndex::SpatialIndex(){
//queue=new priority_queue();
    expand=new Expander();
    // update bounding box
    vcg::tri::UpdateBounding<CMesh>::Box (*mesh);

    // check update topology
    assert(CVertex::HasVFAdjacency());
    assert(CFace::HasFFAdjacency());
    assert(CFace::HasFaceNormal());
    vcg::tri::UpdateTopology<CMesh>::VertexFace (*mesh);
    vcg::tri::UpdateTopology<CMesh>::FaceFace (*mesh);

    // update Normals
    vcg::tri::UpdateNormals<CMesh>::PerVertexNormalizedPerFace (*mesh);
    vcg::tri::UpdateNormals<CMesh>::PerFaceNormalized (*mesh);

    wrap.m = mesh;
    wrap.Update();
    //updateGL ();
    //printf("-%p-\n",&mesh);
    //void * io=&mesh;

    enclosingCluster=tri::Allocator<CMesh>::AddPerVertexAttribute<Cluster*>((*wrap.m), std::string("enclosingCluster"));
    expand->setMesh(mesh);
    build(mesh,10);
}


void SpatialIndex::build(CMesh *mesh, int n){
    topology_index.push_back(*new vector<Cluster>);
    topology_index[0] = base(mesh);
    //printf("\n fine \n");
    for(int i=1;i<n;++i){
        topology_index.push_back(*new vector<Cluster>);
        topology_index[i]=step(topology_index[i-1]);
        printf("livello %d\n",i);
    }
}


vector<Cluster> SpatialIndex::base(CMesh *mesh){//rifare
    //for (int i=0; i<mesh->vert.size(); i++) {
    vector<Cluster> res;
    for(CMesh::VertexIterator vi = mesh->vert.begin(); vi!=mesh->vert.end(); ++vi){
        //vertices[i].
        //printf("\n nel costruttore1--------------------------------------- \n");
        Cluster newCluster;

        newCluster.sphere_radius=0;

        newCluster.sphere_center=Point3d(0,0,0);

        PVertex myVertex;

        myVertex.setVertex(*vi);

        myVertex.setCluster(newCluster);

        enclosingCluster[vi]=&newCluster;

        //myVertices=&(mesh->vert);

        newCluster.vertices.push_back(&myVertex);

        vector<CVertex*> neighbors;
        expand->ring(&*vi,&neighbors);
        //topology_index
        //vector<vector<Cluster> > top;
        //vector<Cluster> vect=topology_index[0];
        //cout << "vect is: " << typeid(top[0]).name() << '\n';
        res.push_back(newCluster);
        //topology_index[0].push_back(newCluster);
        //printf("\n nel costruttore2--------------------------------------- \n");


    }
    return res;

}

vector<Cluster> SpatialIndex::step(vector<Cluster> level){
    priority_queue<Cluster> myQueue=create(level);//chiedere
    vector<Cluster> nextLevel;

    nextLevel=select(myQueue);

    nextLevel=link(nextLevel);

    nextLevel=miniball(nextLevel);
    printf("\n ------ECOOOOOOOOOOOOO------ \n");
    return nextLevel;
}


priority_queue<Cluster> SpatialIndex::create (vector<Cluster> level){
    for(vector<Cluster>::iterator it = level.begin();it!=level.end();it++){
        it->mark=false;
        Cluster myCluster;
        myCluster.sons.push_back(&*it);
        for(vector<Cluster*>::iterator iter=it->neighborhood.begin();iter!=it->neighborhood.end();iter++){
            myCluster.sons.push_back(*iter);
        }
        priorityQueue.push(myCluster);
    }
    return priorityQueue;

}

vector<Cluster> SpatialIndex::select (priority_queue<Cluster> myQueue){
    vector<Cluster> nextLevel;
    while (!myQueue.empty())    {
      bool sons_eliminated = false;
      //printf("%d<<<<----\n",myQueue.size());
      Cluster c= myQueue.top();
      myQueue.pop();
      //printf("%d<<<<----\n",myQueue.size());
      /*for (vector<Cluster*>::iterator CSon=c.sons.begin();CSon!=c.sons.end();CSon++){
          printf("%p----",*CSon);
      }*/
      for (vector<Cluster*>::iterator CSon=c.sons.begin();CSon<c.sons.end();CSon++){
            if ((*CSon)->mark){
              if(!c.sons.empty()){
                CSon=c.sons.erase(CSon);
                //CSon--;
              }
              /*if(CSon==c.sons.begin())
                  printf("begin\n");
              if(CSon==c.sons.end())
                  printf("end\n");*/
              sons_eliminated = true;
              //CSon--;
            }
      }
      if (sons_eliminated){
          if (c.sons.size() != 0){
          myQueue.push(c);
          }
      }
      else{
        nextLevel.push_back(c);
        for (vector<Cluster*>::iterator CSon=c.sons.begin();CSon!=c.sons.end();CSon++)
          (*CSon)->mark = true;
      }
    }
    return nextLevel;
}

vector<Cluster> SpatialIndex::link (vector<Cluster> level){
    for (vector<Cluster>::iterator Clust=level.begin();Clust!=level.end();Clust++){
        for (vector<Cluster*>::iterator CSon=Clust->sons.begin();CSon!=Clust->sons.end();CSon++){
            Clust->vertices.insert(Clust->vertices.end(),(*CSon)->vertices.begin(),(*CSon)->vertices.end());
            (*CSon)->father=&*Clust;
            for (vector<Cluster*>::iterator CNeig=(*CSon)->neighborhood.begin();CNeig!=(*CSon)->neighborhood.end();CNeig++){
                if((*CNeig)->father!=&*Clust && find(Clust->neighborhood.begin(), Clust->neighborhood.end(), (*CNeig)->father)==Clust->neighborhood.end()){
                    Clust->neighborhood.push_back((*CNeig)->father);
                }

            }
        }

    }
    return level;
}

vector<Cluster> SpatialIndex::miniball (vector<Cluster> level){
    for (vector<Cluster>::iterator Clust=level.begin();Clust!=level.end();Clust++){
        //vector<CVertex*> a=NULL;
        MinSphere sphere=MinSphere::computeVertices(Clust->vertices);
        Clust->sphere_radius=sphere.radius;
    }
    return level;
}

//vector<CVertex*>::iterator it=searchCluster->neighborhood.begin();it!=searchCluster->neighborhood.end();it++
vector<CVertex> SpatialIndex::query(CVertex v, float radius){
    printf("\n in query--------------------------------------- \n");
    Cluster* searchCluster=enclosingCluster[v];
    printf("cluster assegnato\n");
    while(distance(v.P(),searchCluster->sphere_center)+searchCluster->sphere_radius<radius && searchCluster->sphere_radius<radius)
        searchCluster=searchCluster->father;
    vector<CVertex*> neighbors;
    vector<CVertex> result;
    printf("qui arriva\n");
    expand->sphereBreadth (&v, radius, 10, &neighbors);
    int i=0;
    for(vector<Cluster*>::iterator it=searchCluster->neighborhood.begin(); it != searchCluster->neighborhood.end();it++){
        for(vector<CVertex*>::iterator iter=(*it)->vertices.begin();iter!=searchCluster->neighborhood[i]->vertices.end();iter++){
            result.push_back(**iter);
        }

    }
    expand->Expander::sphereSpace (&v, radius, &neighbors);

    return result;
}



double SpatialIndex::distance(Point3<float> p1,Point3d p2){
    return sqrt(pow(p1.X()-p2.X(),2)+pow(p1.Y()-p2.Y(),2)+pow(p1.Z()-p2.Z(),2));
}

